//DengFOC V0.2
//灯哥开源，遵循GNU协议，转载请著名版权！
//GNU开源协议（GNU General Public License, GPL）是一种自由软件许可协议，保障用户能够自由地使用、研究、分享和修改软件。
//该协议的主要特点是，要求任何修改或衍生的作品必须以相同的方式公开发布，即必须开源。此外，该协议也要求在使用或分发软件时，必须保留版权信息和许可协议。GNU开源协议是自由软件基金会（FSF）制定和维护的一种协议，常用于GNU计划的软件和其他自由软件中。
//仅在DengFOC官方硬件上测试过，欢迎硬件购买/支持作者，淘宝搜索店铺：灯哥开源
//你的支持将是接下来做视频和持续开源的经费，灯哥在这里先谢谢大家了

#include "DengFOC.h"

int Sensor_DIR=1;    //传感器方向
int Motor_PP=7;    //电机极对数

//小星星整首的音符频率 (C大调，简化版)
float melody[] = {
  261.63,5000, 261.63,5000, 392.00,5000,392.00,5000,440.00,5000, 440.00,5000, 392.00,5000,
  349.23,5000, 349.23,5000, 329.63,5000,329.63,5000,329.63,5000, 293.66,5000, 293.66,5000, 
  392.00,5000, 392.00,5000, 349.23,5000,349.23,5000,329.63,5000, 329.63,5000, 293.66,5000, 
  392.00,5000, 392.00,5000, 349.23,5000,349.23,5000,329.63,5000, 329.63,5000, 293.66,5000, 
  261.63,5000, 261.63,5000, 392.00,5000,392.00,5000,440.00,5000, 440.00,5000, 392.00,5000,
  349.23,5000, 349.23,5000, 329.63,5000,329.63,5000,329.63,5000, 293.66,5000, 293.66,5000, 261.63,5000
};

// 小星星整首的音符持续时间 (单位：毫秒)
int duration[] = {
  500,100, 500,100, 500,100, 500,100, 500,100, 500,100, 500,100,
  500,100, 500,100, 500,100, 500,100, 500,100, 500,100, 500,100,
  500,100, 500,100, 500,100, 500,100, 500,100, 500,100, 500,100,
  500,100, 500,100, 500,100, 500,100, 500,100, 500,100, 500,100,
  500,100, 500,100, 500,100, 500,100, 500,100, 500,100, 500,100,
  500,100, 500,100, 500,100, 500,100, 500,100, 500,100, 500,100,500,100
};

/*
// 云宫迅音整首的音符频率 (C大调，简化版)
float melody[] = {
  220.00,220.00,174.61,196.00,261.63,220.00,174.61,196.00,
  220.00,220.00,174.61,196.00,261.63,220.00,174.61,196.00,
  220.00,220.00,174.61,196.00,261.63,220.00,174.61,196.00,
  220.00,220.00,174.61,196.00,261.63,220.00,174.61,196.00,
  440.00,329.63,329.63,440.00,523.25,
  440.00,329.63,329.63,440.00,523.25,
  440.00,329.63,329.63,440.00,523.25,
  587.33,349.23,440.00,523.25,
  587.33,440.00,523.25,587.33,
  698.46,523.25,587.33,698.46,
  659.25,
  329.63,440.00,493.88,
  523.25,440.00,698.46,587.33,
  659.25,
  659.25,
  329.63,440.00,493.88,
  523.25,440.00,698.46,659.25,
  587.33,
  587.33,
};

// 云宫迅音整首的音符持续时间 (单位：毫秒)
int duration[] = {
  500, 500, 500, 500, 800, 500, 500, 500,
  500, 500, 500, 500, 800, 500, 500, 500,
  500, 500, 500, 500, 800, 500, 500, 500,
  500, 500, 500, 500, 800, 500, 500, 500,
  500, 750, 500, 500, 800,
  500, 750, 500, 500, 800,
  500, 750, 500, 500, 800,
  1000, 500, 500, 800,
  500, 500, 800, 500,
  500, 500, 500, 800,
  1000,
  750, 500, 500,
  800, 500, 500, 800,
  1000,
  1000,
  750, 500, 500,
  800, 500, 500, 800,
  1000,
  1000,
};
*/

unsigned long now_us=0;
float open_loop_timestamp=0;
float Ms_Music=0;
int noteIndex=0;
int freq=3;//频率放大倍数

float Kp;
float Ki;
float Kd;

void setup() {
  Serial.begin(115200);
  DFOC_Vbus(12.6);   //设定驱动器供电电压
  DFOC_alignSensor(Motor_PP,Sensor_DIR);
  DFOC_enable();
  ledcWriteTone(0,melody[0]*freq);
  ledcWriteTone(1,melody[0]*freq);
  ledcWriteTone(2,melody[0]*freq);
}


void loop() 
{
  //设置速度环PID
  DFOC_M0_SET_VEL_PID(0.15,0,0,10000);
  //设置速度
  DFOC_M0_setVelocity(5);

  now_us = micros();
  //计算歌曲音符所用时间记录,单位毫秒
  Ms_Music+=(now_us - open_loop_timestamp) * 1e-3f;
  open_loop_timestamp = now_us;  //用于计算下一个时间间隔

  if(Ms_Music>duration[noteIndex])//对应播放时间
  {
    Ms_Music=0;     //音符播放完重新计时
    noteIndex++;
    
    if (noteIndex >= sizeof(melody) / sizeof(melody[0])) {  //循环播放
    noteIndex = 0;
    }

    ledcWriteTone(0,melody[noteIndex]*freq);
    ledcWriteTone(1,melody[noteIndex]*freq);
    ledcWriteTone(2,melody[noteIndex]*freq);
  }
}
